Skip to content


ai  101  pytorch  classification  nvidia  cuda  install  tensorrt  yolo  ardupilot  None  ros2  dds  micro ros  xrce  sitl  plugin  SITL  debug  rangefinder  pymavlink  mavros  gazebo  distance sensor  system_time  timesync  cmake  gtest  ctest  cpp  c++  format  fmt  multithreading  spdlog  camera  coordinate system  orb  matching  opencv  build  transformation  computer vision  homography  optical flow  of  trackers  cv  cyclonedds  eprosima  fastdds  simulation  config  ignition  bridge  sdf  tips  ign-transport  sensors  lidar  aptly  apt  encryption  pgp  docker  git  bundle  github  hooks  pre-commit  lxd  container  lxc  x11  profile  vscode  marpit  presentation  marp  markdown  mermaid  video  ffmpeg  gstreamer  cheat-sheet  sdp  v4l2loopback  gi  snippets  cheat Sheet  python  asyncio  future  click  cli  numpy  project  template  black  isort  docs  project document  docstrings  flake8  linter  git-hook  mypy  unittest  pytest  pylint  mock  iterator  generator  logging  tuple  namedtuple  typing  annotation  pyzmq  zmq  msgpack  action  namespace  remap  control2  ros2_control  gdb  qos  tag  plugins  msg  node  zero-copy  shm  tutorial  algorithm  calibration  diff  pid  dev  colcon  colcon_cd  rpi  arm  qemu  settings  behavior  plot  visualization  debugging  diagnostic  diagnostics  tutorials  gst  math  apm  rat_runtime_monitor  web  rosbridge  vue  binding  discovery  gazebo-classic  launch  spawn  cook  gps  imu  ray  gazebo_ros_ray_sensor  ultrsonic  range  ultrasonic  gazebo classic  wrench  effort  odom  ign  gz  xacro  ros_ign  diff_drive  odometry  joint_state  argument  OpaqueFunction  DeclareLaunchArgument  LaunchConfiguration  tmux  nav  slam  test  rclpy  executor  MultiThreadedExecutor  SingleThreadedExecutor  param  dynamic-reconfigure  service  client  setup.py  package.xml  parameter  parameters  custom  msgs  executers  pub  sub  rqt  rviz  rviz2  pose  marker  tf2  deb  package  setup  local_setup  rosdep  package manager  project settings  vcstool  cross-compiler  nano  texture  tmuxp  rootfs  embedded  zah  linux  rm  ubuntu  ip  ss  network  netstat  snap  deploy  ssh  systemd  mkdocs  extensions  socat  networking  serial  udp  tc  mtu  select  px4  robotics  kalman_filter  kalman  filter  control  todo  vscode-ext  json  yaml  schema  yocto  poky  world  gazebo_ros2_control  position_controller  effort_controller  velocity_controller  urdf  gazebo_ros_force  gazebo_ros_joint_state_publisher  robot_state_publisher  joint_state_publisher  projects  vrx  buoyancy 

Ardupilot MAVROS minimal python node


Table of Content

Implement ROS2 Python node usage mavros That change mode, arm and takeoff using mavros services and topics

SITL#

terminal1
sim_vehicle.py -v ArduCopter --console

mavros#

terminal2
ros2 run  mavros mavros_node --ros-args -p fcu_url:=udp://:14550@

node#

  • set mode to ‘GUIDED’
  • Arming
  • Takeoff
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_system_default
from mavros_msgs.msg import State
from mavros_msgs.srv import SetMode, CommandBool, CommandTOL
# from mavros_msgs.srv._set_mode import SetMode_Request


TOPIC_TAKEOFF = "/mavros/cmd/takeoff"
TOPIC_ARMING = "/mavros/cmd/arming"
TOPIC_SET_MODE = "/mavros/set_mode"
TOPIC_STATE = "/mavros/state"

class MyNode(Node):
    def __init__(self):
        node_name="minimal"
        super().__init__(node_name)
        self.armed = False
        self.in_air = False
        self.state_sub = self.create_subscription(State, TOPIC_STATE, self.state_cb, qos_profile=qos_profile_system_default)
        self.set_mode_client = self.create_client(SetMode, , qos_profile=qos_profile_system_default)
        self.arming_client = self.create_client(CommandBool, TOPIC_ARMING, qos_profile=qos_profile_system_default)
        self.takeoff_client = self.create_client(CommandTOL, TOPIC_TAKEOFF, qos_profile=qos_profile_system_default)
        while not self.set_mode_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('set_mode service not available, waiting again...')
        self.get_logger().info("hello mavros python")

    def mode_changed_handler(self, future):
        response = future.result()
        self.get_logger().info("{}".format(response))

    def arming_handler(self, future):
        response = future.result()
        self.get_logger().info("{}".format(response))
        self.armed = response.success

    def takeoff_handler(self, future):
        response = future.result()
        self.get_logger().info("{}".format(response))
        self.in_air = response.success

    def state_cb(self, msg):
        self.get_logger().info("{}".format(msg), throttle_duration_sec=2)
        if msg.mode == "STABILIZE":
            request = SetMode.Request() # SetMode_Request()
            request.custom_mode = "GUIDED"
            future = self.set_mode_client.call_async(request)
            future.add_done_callback(self.mode_changed_handler)

        if msg.mode == "GUIDED" and not self.armed:
            request = CommandBool.Request()
            request.value = True
            future = self.arming_client.call_async(request)
            future.add_done_callback(self.arming_handler)

        if msg.armed and not self.in_air:
            request = CommandTOL.Request()
            request.altitude = 15.0
            future = self.takeoff_client.call_async(request)
            future.add_done_callback(self.takeoff_handler)


def main(args=None):
    rclpy.init(args=args)
    node = MyNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()